package com.tmrnavi.sensormeasureservice;

/* *
 *       低通滤波器
 *       原来的20Hz的低通滤波器存在问题，修改为简单的滤波器
 * */
public class LPFilter {
    public final int length = 8;       // 滤波器阶数
    public LoopVector xkv = new LoopVector(length);
    public LoopVector ykv = new LoopVector(length);
    public float[][] xk = new float[3][10];
    public float[][] yk = new float[3][10];
    private final float[] NUM = new float[]{
            0.0300994026938011f, 0.210695818856608f, 0.632087456569823f, 1.05347909428304f,
            1.05347909428304f, 0.632087456569823f, 0.210695818856608f, 0.0300994026938011f
//            0.1f, 0,0,0, 0,0,0, 0,0,0
    };
    private final float[] DEN = new float[]{
            1f, 0.795537289167526f, 1.16722517231220f, 0.522578717845529f, 0.288961625831579f,
            0.0645593384576788f, 0.0130273726393524f, 0.000834028552673317f
//            1, -0.9f, 0,0,0, 0,0,0, 0,0
    };
    public float[] update(float[] x){
        float[] res = new float[]{0,0,0};
//        for (int i=0; i<3; i++) {
//            for (int k = 9; k>0; k--) {
//                xk[i][k] = xk[i][k - 1];
//                yk[i][k] = yk[i][k - 1];
//                res[i] += xk[i][k] * NUM[k] - yk[i][k]*DEN[k];
//            }
//            xk[i][0] = x[i];
//            res[i] += xk[i][0] * NUM[0];
//            yk[i][0] = res[i];
//        }

        xkv.write(x);
        xkv.rp = xkv.wp;
        for (int k=length-1; k>=0; k--) {
            float[] xk = xkv.read();
            for (int i=0; i<3; i++)
                res[i] += xk[i]*NUM[k];
        }
        ykv.rp = ykv.wp+1;
        for (int k=length-1; k>0; k--) {
            float[] yk = xkv.read();
            for (int i=0; i<3; i++)
                res[i] -= yk[i]*DEN[k];
        }
        ykv.write(res);

        return res;
    }

    public void clear() {
        for (int i=0; i<3; i++){
            for (int k=0; k<10; k++) {
                xk[i][k] = 0.0f;
            }
        }
    }
}

/*
 *  三维向量环形缓冲区
 */
class LoopVector{
    public float[][] data;          // 数据
    public int length;              // 长度
    public int wp, rp;              // 写指针，读指针

    public LoopVector(int len) {    // 构造函数
        length = len;
        data = new float[length][3];
        wp = rp = 0;
    }

    public float[] read() {         // 读数据
        float[] res =  data[rp++];
        rp = rp>=length ? 0 : rp;
        return res;
    }

    public void write(float[] d) { // 写数据
        data[wp++] = d;
        wp = wp>=length ? 0 : wp;
    }

    public void clear() {           // 清空全部数据
        for (int i=0; i<length; i++)
            for (int j = 0; j < 3; j++)
                data[i][j] = 0;
    }
}
